﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Effects.Filters
{
    /// <summary>
    ///  Fourth order filter with methods to create Butterworth filter.
    /// </summary>
    public class FourthOrderFilter:Filter
    {
        #region coefficients
        // normalized coefficients from transfer functions

        /// <summary>
        ///  Nominator coefficients.
        /// </summary>
        private double[] a;

        /// <summary>
        ///  Gets nominator coefficients from transfer function.
        /// </summary>
        public double[] A
        {
            get { return a; }
        }

        /// <summary>
        ///  Denominator coefficients.
        /// </summary>
        private double[] b;

        /// <summary>
        ///  Gets denominator coefficients from transfer function.
        /// </summary>
        public double[] B
        {
            get { return b; }
        }

        #endregion

        #region previous_inputs_and_outputs
        // previous Xs and Ys of the filter.

        /// <summary>
        ///  Previous inputs. 
        /// </summary>
        private double[] x;

        /// <summary>
        ///  Previous outputs.
        /// </summary>
        private double[] y;

        #endregion

        #region constructor
        // constructors 

        /// <summary>
        ///  Creates new 4th order IIR filter with given normalized coefficients.
        /// </summary>
        /// <param name="a"> Normalized nominator ceofficients. </param>
        /// <param name="b"> Normalized denominator coefficients. </param>
        /// <remarks>
        ///  All coefficients are expected directly from filter's transfer function.
        /// </remarks>
        public FourthOrderFilter(double[] a, double[] b)
        {
            if (a.Length != 5)
                throw new ArgumentException("Array containing nominator coefficients has invalid length. For 2nd order filter expectd length is 3");

            if (b.Length != 5)
                throw new ArgumentException("Array containing denominator coefficients has invalid length. For 2nd order filter expectd length is 3");

            this.a = a;
            this.b = b;

            x = new double[5];
            y = new double[5];
        }

        /// <summary>
        ///  Creates new 4th order IIR filter with given coefficients. If normalized argument is set to false it normalizes them itself.
        /// </summary>
        /// <param name="a"> Nominator coefficients. </param>
        /// <param name="b"> Denominator coefficients. </param>
        /// <param name="normalized"> Indicates whether given coefficients are normalized or not. </param>
        /// <remarks>
        ///  All coefficients are expected directly from filter's transfer function.
        /// </remarks>
        public FourthOrderFilter(double[] a, double[] b, bool normalized):this(a,b)
        {
            if (!normalized)
            {
                for (int i = 0; i < 5; i++)
                    this.a[i] /= this.b[0];

                for (int i = 1; i < 5; i++)
                    this.b[i] /= this.b[0];

                this.b[0] = 1;
            }
        }

        #endregion

        #region step_method
        // overridden step method

        /// <summary>
        ///  Filters input signal.
        /// </summary>
        /// <param name="x0"> Input signal </param>
        /// <returns> Filtered output signal </returns>
        public override double Step(double x0)
        {
            x[0] = x0;
            y[0] = 0;

            y[0] = a[0] * x[0] + a[1] * x[1] + a[2] * x[2] + a[3] * x[3] + a[4] * x[4] - b[1] * y[1] - b[2] * y[2] - b[3] * y[3] - b[4] * y[4];

            // move Xs and Ys
            x[4] = x[3];
            x[3] = x[2];
            x[2] = x[1];
            x[1] = x[0];

            y[4] = y[3];
            y[3] = y[2];
            y[2] = y[1];
            y[1] = y[0];

            return y[0];
        }

        #endregion

        #region creator_methods
        // method creates different types of 2nd order filters

        /// <summary>
        ///  Constant for Butterworth filter denominator.
        /// </summary>
        private const double I = 0.7654;

        /// <summary>
        ///  Constant for Butterworth filter denominator.
        /// </summary>
        private const double J = 1.8478;

        /// <summary>
        ///  Creates 4th order low-pass filter Butterworth filter.
        /// </summary>
        /// <param name="frequency"> Cut-off frequency </param>
        /// <param name="sampleRate"> Sample rate of the input signal </param>
        /// <returns> 4th order low-pass filter with given cut-off frequency </returns>
        public static FourthOrderFilter CreateLowPass(double frequency, int sampleRate)
        {
            // normalized and warp frequency
            double normalizedFreq = frequency / sampleRate * Math.PI * 2;
            double omega = Math.Tan(normalizedFreq / 2);
            double omega2 = omega * omega;
            double omega4 = omega2 * omega2;

            double X1 = omega2 + 1 - I * omega;
            double X2 = 2 * (omega2 - 1);
            double X3 = omega2 + 1 + I * omega;

            double Y1 = omega2 + 1 - J * omega;
            double Y2 = 2 * (omega2 - 1);
            double Y3 = omega2 + 1 + J * omega;

            double[] a = new double[5];
            double[] b = new double[5];

            // prepare coefficients
            a[0] = 1 * omega4;
            a[1] = 4 * omega4;
            a[2] = 6 * omega4;
            a[3] = 4 * omega4;
            a[4] = 1 * omega4;

            b[0] = X3 * Y3;
            b[1] = X2 * Y3 + X3 * Y2;
            b[2] = X1 * Y3 + X2 * Y2 + X3 * Y1;
            b[3] = X1 * Y2 + X2 * Y1;
            b[4] = X1 * Y1;

            // normalize coefficients
            for (int i = 0; i < 5; i++)
                a[i] /= b[0];
            for (int i = 1; i < 5; i++)
                b[i] /= b[0];
            b[0] = 1;

            return new FourthOrderFilter(a, b);
        }

        /// <summary>
        ///  Creates 4th order high-pass filter Butterworth filter.
        /// </summary>
        /// <param name="frequency"> Cut-off frequency </param>
        /// <param name="sampleRate"> Sample rate of the input signal </param>
        /// <returns> 4th order high-pass filter with given cut-off frequency </returns>
        public static FourthOrderFilter CreateHighPass(double frequency, int sampleRate)
        {
            // normalized and warp frequency
            double normalizedFreq = (sampleRate/2 - frequency) / sampleRate * Math.PI * 2;
            double omega = Math.Tan(normalizedFreq / 2);
            double omega2 = omega * omega;
            double omega4 = omega2 * omega2;

            double X1 = omega2 + 1 - I * omega;
            double X2 = 2 * (omega2 - 1);
            double X3 = omega2 + 1 + I * omega;

            double Y1 = omega2 + 1 - J * omega;
            double Y2 = 2 * (omega2 - 1);
            double Y3 = omega2 + 1 + J * omega;

            double[] a = new double[5];
            double[] b = new double[5];

            // prepare coefficients
            a[0] = 1 * omega4;
            a[1] = -4 * omega4;
            a[2] = 6 * omega4;
            a[3] = -4 * omega4;
            a[4] = 1 * omega4;

            b[0] = X3 * Y3;
            b[1] = -1 * (X2 * Y3 + X3 * Y2);
            b[2] = X1 * Y3 + X2 * Y2 + X3 * Y1;
            b[3] = -1 * (X1 * Y2 + X2 * Y1);
            b[4] = X1 * Y1;

            // normalize coefficients
            for (int i = 0; i < 5; i++)
                a[i] /= b[0];
            for (int i = 1; i < 5; i++)
                b[i] /= b[0];
            b[0] = 1;

            return new FourthOrderFilter(a, b);
        }

        /// <summary>
        ///  Creates 4th order band pass filter Butterworth filter.
        /// </summary>
        /// <param name="frequencyLow"> Lower border frequency </param>
        /// <param name="frequencyUp"> Upper border frequency </param>
        /// <param name="sampleRate"> Sample rate of the input signal </param>
        /// <returns> 4th order band pass filter with given border frequency </returns>
        public static FourthOrderFilter CreateBandPass(double frequencyLow,double frequencyUp, int sampleRate)
        {
            // normalize frequencies
            double normalizedLow = frequencyLow / sampleRate * 2 * Math.PI;
            double normalizedUp = frequencyUp / sampleRate * 2 * Math.PI;

            // warp frequencies
            double omegaLow = Math.Tan(normalizedLow / 2);
            double omegaUp = Math.Tan(normalizedUp / 2);

            double A = omegaLow * omegaUp;
            double B = omegaUp - omegaLow;

            double Y1 = Math.Sqrt(2) * B;
            double Y2 = 2 * A + B * B;
            double Y3 = Y1 * A;
            double Y4 = A * A;

            double[] a = new double[5];
            double[] b = new double[5];

            a[0] = B * B;
            a[1] = 0;
            a[2] = -2 * B * B;
            a[3] = 0;
            a[4] = a[0];

            b[0] = 1 + Y1 + Y2 + Y3 + Y4;
            b[1] = -4 - 2 * Y1 + 2 * Y3 + 4 * Y4;
            b[2] = 6 - 2 * Y2 + 6 * Y4;
            b[3] = -4 + 2 * Y1 - 2 * Y3 + 4 * Y4;
            b[4] = 1 - Y1 + Y2 - Y3 + Y4;

            return new FourthOrderFilter(a, b,false);
        }

        #endregion
    }
}
